#include "ms618_algsidese7edge.h"

using namespace MeasurE618;
AlgSideSE7Edge::AlgSideSE7Edge(int i):AlgBase(i)
{

}

void AlgSideSE7Edge::operator()(cv::Mat &srcImg,
                                int id,
                                std::map<string, float> &measRes,
                                int fixtureId)
{
//    qDebug() << " ";
//side 图    SE7
    //    int LDmeasurerevise(cv::Mat srcimg, const vector<int> paras, vector<cv::Point2f>& respts);

    std::vector<std::vector<cv::Point2f>> resptsVec;
    resptsVec.resize(2);
    {
        std::vector<int> tparas;
        int i=34;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(0,j));
        }
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();

        std::vector<cv::Point2f> respts;
        edgedetect obj;
        obj.LDmeasurerevise(imgt,tparas,respts);

        ImageCoordinateTransformation(respts,rangVec);

        std::unique_lock<std::mutex> lock(*AlgBase::mtxptrsideVec[id]);
        while (!AlgBase::flagsideVec[id])
        {
            (*AlgBase::cvarptrsideVec[id]).wait(lock);
        }

        for(auto &elem:respts)
        {
            cv::Point2f ptsOut;
//            CoordinateTransformation(datumSideVec[id],elem,ptsOut);
            CoordinateTransformationRelateToDot(datumSideVec[id], elem, ptsOut);
            resptsVec[0].push_back(ptsOut);
        }
    }

    {
        std::vector<int> tparas;
        int i=34;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(1,j));
        }
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point2f> respts;
        edgedetect obj;
        obj.LDmeasurerevise(imgt,tparas,respts);

        ImageCoordinateTransformation(respts,rangVec);

        std::unique_lock<std::mutex> lock(*AlgBase::mtxptrsideVec[id]);
        while (!AlgBase::flagsideVec[id])
        {
            (*AlgBase::cvarptrsideVec[id]).wait(lock);
        }

        for(auto &elem:respts)
        {
            cv::Point2f ptsOut;
//            CoordinateTransformation(datumSideVec[id],elem,ptsOut);
            CoordinateTransformationRelateToDot(datumSideVec[id], elem, ptsOut);
            resptsVec[1].push_back(ptsOut);
        }
    }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";

    if(resptsVec[0].size()==3){
        measRes.insert(std::make_pair(string("FAI36-SE7-Y1-distance_pixel"),resptsVec[0][0].y));
        float fai36se7y1 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI36-SE7-Y1-distance")], resptsVec[0][0].y);
        measRes.insert(std::make_pair(string("FAI36-SE7-Y1-distance_real"), fai36se7y1));

        measRes.insert(std::make_pair(string("FAI36-SE7-Y2-distance_pixel"),resptsVec[0][1].y));
        float fai36se7y2 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI36-SE7-Y2-distance")], resptsVec[0][1].y);
        measRes.insert(std::make_pair(string("FAI36-SE7-Y2-distance_real"), fai36se7y2));

        measRes.insert(std::make_pair(string("FAI36-SE7-Y3-distance_pixel"),resptsVec[0][2].y));
        float fai36se7y3 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI36-SE7-Y3-distance")], resptsVec[0][2].y);
        measRes.insert(std::make_pair(string("FAI36-SE7-Y3-distance_real"), fai36se7y3));
    }

    if(resptsVec[1].size()==6) {
        measRes.insert(std::make_pair(string("FAI37-SE7-Y1-distance_pixel"),resptsVec[1][0].y));
        float fai37se7y1 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI37-SE7-Y1-distance")], resptsVec[1][0].y);
        measRes.insert(std::make_pair(string("FAI37-SE7-Y1-distance_real"), fai37se7y1));

        measRes.insert(std::make_pair(string("FAI37-SE7-Y2-distance_pixel"),resptsVec[1][2].y));
        float fai37se7y2 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI37-SE7-Y2-distance")], resptsVec[1][2].y);
        measRes.insert(std::make_pair(string("FAI37-SE7-Y2-distance_real"), fai37se7y2));

        measRes.insert(std::make_pair(string("FAI37-SE7-Y3-distance_pixel"),resptsVec[1][3].y));
        float fai37se7y3 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI37-SE7-Y3-distance")], resptsVec[1][3].y);
        measRes.insert(std::make_pair(string("FAI37-SE7-Y3-distance_real"), fai37se7y3));

        measRes.insert(std::make_pair(string("FAI37-SE7-Y4-distance_pixel"),resptsVec[1][5].y));
        float fai37se7y4 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI37-SE7-Y4-distance")], resptsVec[1][5].y);
        measRes.insert(std::make_pair(string("FAI37-SE7-Y4-distance_real"), fai37se7y4));

        measRes.insert(std::make_pair(string("FAI38-SE7-Y1-distance_pixel"),fabs(resptsVec[1][0].y-resptsVec[1][1].y)));
        float fai38se7y1 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI38-SE7-Y1-distance")], fabs(resptsVec[1][0].y-resptsVec[1][1].y));
        measRes.insert(std::make_pair(string("FAI38-SE7-Y1-distance_real"), fai38se7y1));

        measRes.insert(std::make_pair(string("FAI38-SE7-Y2-distance_pixel"),fabs(resptsVec[1][2].y-resptsVec[1][1].y)));
        float fai38se7y2 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI38-SE7-Y2-distance")], fabs(resptsVec[1][2].y-resptsVec[1][1].y));
        measRes.insert(std::make_pair(string("FAI38-SE7-Y2-distance_real"), fai38se7y2));

        measRes.insert(std::make_pair(string("FAI38-SE7-Y3-distance_pixel"),fabs(resptsVec[1][3].y-resptsVec[1][4].y)));
        float fai38se7y3 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI38-SE7-Y3-distance")], fabs(resptsVec[1][3].y-resptsVec[1][4].y));
        measRes.insert(std::make_pair(string("FAI38-SE7-Y3-distance_real"), fai38se7y3));

        measRes.insert(std::make_pair(string("FAI38-SE7-Y4-distance_pixel"),fabs(resptsVec[1][5].y-resptsVec[1][4].y)));
        float fai38se7y4 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI38-SE7-Y4-distance")], fabs(resptsVec[1][5].y-resptsVec[1][4].y));
        measRes.insert(std::make_pair(string("FAI38-SE7-Y4-distance_real"), fai38se7y4));
    }
}
